#ifndef SCORBOT_H
#define SCORBOT_H

#include "queue.h"

/* Common definitions */

// This is a description of an objects position/orientation
typedef struct poDesc_t {
    double x, y, z;
    double phi, rho, theta;
} poDesc_t;

typedef struct{
	double e[4][4];
} H_Matrix; // Transformation matrix
typedef struct{
	double v[4];
} T_Matrix;// Translation vector
typedef double R_Matrix[3][3]; // Rotation matrix

typedef enum {
    BLOCK,
    AREA
} IMAGE_TARGET;

/* 
   Move driver prototypes 

   These functions should block until the motion is complete.
   This simplifies the task of the controller piece, which
   handles overseeing everything (managing movement and image
   processing
*/
#if defined(__cplusplus)
extern "C" {
#endif

int driver_init();
int open_gripper();
int close_gripper();
int scorbot_MoveAbs(poDesc_t *pos, int wait);
int scorbot_MoveC(char* name, int timeout);
int setSpeed(int);
int commDriver_Init();
void commDriver_Uninit();

poDesc_t makePoFromScorbotXYZPR(double X, double Y, double Z, double P, double R);
void makeHB2GFromPo(poDesc_t po, H_Matrix *h_b2g);
void makeHG2BFromHB2G(H_Matrix *h_b2g, H_Matrix *h_g2b);

#if defined(__cplusplus)
}
#endif

#endif
